package legway;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
import lejos.nxt.LightSensor;
import lejos.nxt.Sound;
import lejos.robotics.subsumption.Behavior;

/**
 * A behavior that makes the Legway follow a black line on a white surface. 
 * 
 * @author Kim Petersen, Andreas Koefoed-Hansen og Tim Rasmussen
 * @version Januar 2010
 */
public class FollowLine implements Behavior {

	private static int blackWhiteThreshold = 45;
	private boolean isSuppressed;
	private static LightSensor leftLight;
	private static LightSensor rightLight;

	private Car car;
	private CtrlParam ctrl;

	private static boolean isEnabled;

	/**
	 * @return isEnabled Whether or not the behavior is enabled.
	 */
	public static  boolean isEnabled() {
		return isEnabled;
	}

	/**
	 * @param isEnabled Enable/disable the behavior.
	 */
	public static synchronized void setEnabled(boolean isEnabled) {
		FollowLine.isEnabled = isEnabled;
	}

	/**
	 * The FollowLine constructor. 
	 * @param ctrl The motor control parameters.
	 */
	public FollowLine(CtrlParam ctrl) {
		leftLight = new LightSensor(SensorPort.S4);
		rightLight = new LightSensor(SensorPort.S1);
		leftLight.setFloodlight(false);
		rightLight.setFloodlight(false);
		this.ctrl = ctrl;
		car = new Car(ctrl);
	}

	/**
	 * Action methods, which make the Legway folow the black line. 
	 */
	public void action() {
		isSuppressed = false;
		leftLight.setFloodlight(true);
		rightLight.setFloodlight(true);
		car.slowForward();
		boolean isLeftBlack;
		boolean isRightBlack;
		while (!isSuppressed) {
			isLeftBlack = leftLight.readValue() < blackWhiteThreshold;
			isRightBlack = rightLight.readValue() < blackWhiteThreshold;

			if (isLeftBlack && isRightBlack) { // Black/black
				ctrl.setLeftMotorOffset(0);
				ctrl.setRightMotorOffset(0);
				car.slowForward();
			}
			else if (isLeftBlack) { // Left = black
				ctrl.setLeftMotorOffset(-160);
				ctrl.setRightMotorOffset(0);
			}
			else if (isRightBlack) { // Right = black
				ctrl.setLeftMotorOffset(0);
				ctrl.setRightMotorOffset(-160);
			}
			//LCD.drawInt(isRightBlack ? 1 : 0, 0, 7);
			//LCD.drawInt(isLeftBlack ? 1 : 0, 12, 7);
			try {
				Thread.sleep(10);
			} catch (InterruptedException e) { }
		}
		car.stop();
		leftLight.setFloodlight(false);
		rightLight.setFloodlight(false);
	}

	/**
	 * Stop following the line.
	 */
	public void suppress() {
		isSuppressed = true;
	}

	/**
	 * Always try to take control if the behavior is enabled.
	 */
	public boolean takeControl() {
		return isEnabled;
	}

	/**
	 * Calibrate the black/white threshold.
	 */
	public static void calcThreshold() throws Exception {
		int whiteSum = 0;
		int blackSum = 0;
		LCD.clear();
		rightLight.setFloodlight(true);
		leftLight.setFloodlight(true);
		LCD.drawString("Place light", 0, 1);
		LCD.drawString("sensors on WHITE", 0, 2);
		LCD.drawString("surface and", 0, 3);
		LCD.drawString("press enter", 0, 4);
		Thread.sleep(400);
		Button.ENTER.waitForPressAndRelease();
		for(int i = 0; i < 10; i++) {
			whiteSum += leftLight.readValue() + rightLight.readValue();
			Thread.sleep(10);
		}
		Sound.beep();
		LCD.drawString("Place light", 0, 1);
		LCD.drawString("sensors on BLACK", 0, 2);
		LCD.drawString("surface and", 0, 3);
		LCD.drawString("press enter", 0, 4);
		Button.ENTER.waitForPressAndRelease();
		for(int i = 0; i < 10; i++) {
			blackSum += leftLight.readValue() + rightLight.readValue();
			Thread.sleep(10);
		}
		Sound.twoBeeps();
		blackWhiteThreshold = (int) Math.round((whiteSum + blackSum)/40.0);
		rightLight.setFloodlight(false);
		leftLight.setFloodlight(false);
	}
}
